# main.py -- put your code here!
import pyb
from pyb import *
# from pyb import Pin
# from pyb import Timer
# from pyb import Servo
import time

class AGV():
    def __init__(self):
        # 点击控制 PIN  N1, N2左轮, N3, N4 右轮
        self.N1 = Pin('Y1', Pin.OUT_PP)
        self.N2 = Pin('Y2', Pin.OUT_PP)
        self.N3 = Pin('Y3', Pin.OUT_PP)
        self.N4 = Pin('Y4', Pin.OUT_PP)
        # 超声波模块
        self.Trig = Pin('Y7', Pin.OUT_PP)  # 超声波启动信号
        self.Echo = Pin('Y8', Pin.IN)  # 超声波信号接收
        # 舵机对象
        self.servo1 = pyb.Servo(1)

        # 限速
        self.speed1 = 0.07
        self.speed2 = 0
        # 最大距离
        self.max1 = 40

        self.num=0
        self.flag=0
        self.run=1
        self.zuo=0
        self.qian=0
        self.you=0
        self.distance = 0
        # 中断回掉函数启动
        self.start_stop_run()

    # 内存返还
    def __del__(self):
        self.N1.low()
        self.N2.low()
        self.N3.low()
        self.N4.low()
        self.servo1.angle(0, 500)
        self.Trig.value(0)
        self.num=0
        self.flag=0
        self.run=1
        self.zuo=0
        self.qian=0
        self.you=0
        self.distance=0
        pyb.LED(3).off()

    # 超声波测距离计时
    def start(self, t):
        if(self.flag==0):
            self.num = 0
        else:
            self.num = self.num + 1
    # 计时结束
    def stop(self, t):
        if(self.run==0):
            self.run=1

    # 启动中断
    def start_stop_run(self):
        start1 = Timer(1, freq=10000, callback=self.start)
        stop1 = Timer(4, freq=2, callback=self.stop)

    #小车停止
    def stop_AGV(self):
        self.N1.low()
        self.N2.low()
        self.N3.low()
        self.N4.low()

    #小车左转
    def left(self):
        print("小车左转>>")
        self.N1.low()
        self.N2.low()
        self.N3.low()
        self.N4.high()

    #小车前进
    def go(self):
        print("直行>>直行>>直行>>")
        # 左轮直行
        self.N1.low()
        self.N2.high()
        # 右轮直行
        self.N3.low()
        self.N4.high()

        time.sleep(self.speed1)  # 这里时间差越大, 速度越慢
        self.stop_AGV()
        time.sleep(self.speed2)

    #小车后退
    def back(self):
        print("小车后退>>")
        # 左轮后退
        self.N1.high()
        self.N2.low()
        # 右轮后退
        self.N3.high()
        self.N4.low()

    #小车右转
    def right(self):
        print("小车右转>>")
        self.N1.low()
        self.N2.high()
        self.N3.low()
        self.N4.low()


    #控制舵机向右、向左、向前
    def servo(self):
        try:
            #向右75旋转
            self.servo1.angle(-75, 500)
            pyb.delay(1000)
            you = self.ceju()  # 超声运行
            print('右边距离: ', you)

            #向左转75度
            self.servo1.angle(75, 500)
            pyb.delay(1000)
            zuo = self.ceju()  # 超声运行
            print('左边距离: ', zuo)

            #转到0度
            self.servo1.angle(0, 500)
            pyb.delay(1000)
            qian = self.ceju()  # 超声运行
            print('前方距离: ', qian)

            #返回正前方、左前方、右前方的距离
            return qian, zuo, you   # 返回前左右3个方向物体距离
        except Exception as e:
            self.__del__()
            print("舵机异常: ", e)
            return 0, 0, 0

    #测距方法
    def ceju(self):
        try:
            if(self.run==1):
                # 超声波开始工作
                self.Trig.value(1)
                pyb.udelay(100)
                self.Trig.value(0)
                # 检查到反射信号
                while(self.Echo.value()==0):
                    self.Trig.value(1)
                    pyb.udelay(100)
                    self.Trig.value(0)
                    self.flag = 0
                # 开始统计时间距离
                if(self.Echo.value()==1):
                    self.flag=1
                    while(self.Echo.value()==1):
                        self.flag = 1
                if(self.num != 0):
                    #测距 用时间计算距离公式  点位CM
                    self.distance = self.num/10000*34299/2
                    #print('Distance:')
                    #print(distance,'cm')
                    # pyb.delay(100)
                self.flag = 0
                self.run = 0
                return self.distance  # 返回测试距离结果
        except Exception as e:
            print("超声波测距离错误")
            self.__del__()
            return 0

    # 程序执行入口
    def main(self):
        try:
            self.servo1.angle(0, 500)  # 舵机重置  返回角度0
            pyb.delay(1000)
            self.ceju()    # 超声波测距离开始工作
            while True:
                self.ceju()   # 循环测距前方距离
                #前方距离大于40cm前进
                if self.distance > self.max1:
                    self.go()
                    self.ceju()  # 测距离
                    print('前方物体距离', self.distance)

                #前方距离小于40cm
                if self.distance <= self.max1:
                    self.stop_AGV()   # 停止
                    self.back()  # 后退
                    pyb.delay(100)  # 时间越小后台距离越越大
                    self.stop_AGV()
                    self.qian, self.zuo, self.you = self.servo()  # 启动舵机 进行3个方向距离测量

                    #如果右前方距离大于左前方
                    if self.you > self.zuo:
                        print("右方障碍易于通行!!>>右转")
                        self.right()  # 右转
                        pyb.delay(100)
                        self.ceju()
                    #如果左前方距离大于右前方
                    if self.zuo > self.you:
                        print("左方障碍易于通行!!>>左转")
                        self.left()
                        pyb.delay(100)
                        self.ceju()   # 获取前方障碍距离
                    #如果所有方向距离全部小于15cm
                    if self.zuo < 25 and self.you < 25 and self.qian < 25:
                        self.stop_AGV()
                        self.back()
                        pyb.delay(100)
                        self.stop_AGV()
                        self.ceju()
                    #有一个方向距离小于5CM
                    if self.zuo < 15 or self.you < 15 or self.qian < 15:
                        self.stop_AGV()
                        self.back()
                        pyb.delay(100)
                        self.stop_AGV()
                        self.ceju()

                pyb.delay(30)
        except:
            self.__del__()


if __name__=='__main__':
    print(">>>>>>>>>>>004\n开始运行>>>>>>>")
    pyb.LED(3).on()
    obj = AGV()
    try:
        obj.main()
    except:
        obj.__del__()
        pyb.LED(3).off()
        print('程序退出!!')